from DebugTrace import DebugTrace
from math import *
from time import sleep
from ShiftRegister import *
from fractions import gcd

class MotorDriver:
    def __init__(self, stepsPerMm, debugLevel=1):
        self.tool=0
        self.stepsPerMm = stepsPerMm
        self.motors= {"X":None, "Y":None, "Z":None, "E0":None, "E1":None}
        self.trace = DebugTrace("MotorDriver",debugLevel)
        self.outputSR = None
        self.inputSR = None
        self.motorPos = {"X":0.0, "Y":0.0, "Z":0.0, "E0":0.0, "E1":0.0}
        self.io = wiringpi.GPIO(wiringpi.GPIO.WPI_MODE_SYS)

    def initStepPins(self):
        if (self.motors["X"]!=None):
            self.io.pinMode(self.motors["X"]["Step"],self.io.OUTPUT)
            self.io.digitalWrite(self.motors["X"]["Step"],self.io.LOW)
        if (self.motors["Y"]!=None):
            self.io.pinMode(self.motors["Y"]["Step"],self.io.OUTPUT)
            self.io.digitalWrite(self.motors["Y"]["Step"],self.io.LOW)
        if (self.motors["Z"]!=None):
            self.io.pinMode(self.motors["Z"]["Step"],self.io.OUTPUT)
            self.io.digitalWrite(self.motors["Z"]["Step"],self.io.LOW)
        if (self.motors["E0"]!=None):
            self.io.pinMode(self.motors["E0"]["Step"],self.io.OUTPUT)
            self.io.digitalWrite(self.motors["E0"]["Step"],self.io.LOW)
        if (self.motors["E1"]!=None):
            self.io.pinMode(self.motors["E1"]["Step"],self.io.OUTPUT)
            self.io.digitalWrite(self.motors["E1"]["Step"],self.io.LOW)

    def getDistanceToPosition(self,motor,pos):
        if (pos!=None):
            return float(pos)-self.motorPos[motor]

    def setPosition(self,motor,pos):
        if (pos!=None):
            self.motorPos[motor]=pos

    def getCurrentToolMotor(self):
        if (self.tool==0):
            self.trace.trace(3,"Tool 0")
            return "E0"
        elif (self.tool==1):
            self.trace.trace(3,"Tool 1")
            return "E1"
        else:
            self.trace.trace(1,"Invalid tool number "+str(self.tool))
            return None #Failure

    def addOutputSR(self,output):
            self.outputSR=output

    def addInputSR(self,inputSR):
            self.inputSR = inputSR

    def setTool(self,tool):
        if (tool==0 or tool==1):
            self.tool=tool
            self.trace.trace(2,"Setting tool to "+str(tool))
            return "ok"
        else:
            self.trace.trace(1,"Invalid tool number "+str(tool))
            return "!!"

    def step(self,motor):
        if (self.motors[motor]!=None):
            #Double writes are intended to just waste a little bit of time
            #self.io.digitalWrite(self.motors[motor]["Step"],self.io.HIGH)
            self.io.digitalWrite(self.motors[motor]["Step"],self.io.HIGH)
            #self.io.digitalWrite(self.motors[motor]["Step"],self.io.LOW)
            self.io.digitalWrite(self.motors[motor]["Step"],self.io.LOW)         

    def releaseMotors(self):
        self.outputSR.setPinValue(self.motors["Y"]["En"],True)
        self.outputSR.setPinValue(self.motors["X"]["En"],True)
        self.outputSR.setPinValue(self.motors["Z"]["En"],True)
        if (self.motors["E0"]!=None):
            self.outputSR.setPinValue(self.motors["E0"]["En"],True)
        if (self.motors["E1"]!=None):
            self.outputSR.setPinValue(self.motors["E1"]["En"],True)
        self.outputSR.shiftAll()
        
    def holdMotors(self):
        self.outputSR.setPinValue(self.motors["Y"]["En"],False)
        self.outputSR.setPinValue(self.motors["X"]["En"],False)
        self.outputSR.setPinValue(self.motors["Z"]["En"],False)
        if (self.motors["E0"]!=None):
            self.outputSR.setPinValue(self.motors["E0"]["En"],False)
        if (self.motors["E1"]!=None):
            self.outputSR.setPinValue(self.motors["E1"]["En"],False)
        self.outputSR.shiftAll()

    def move(self,x,y,z,e):  #Pin list names are "En", "Dir", "Step"
        self.trace.trace(2,"Moving to X:"+str(x)+" Y:"+str(y)+" Z:"+str(z)+" E:"+str(e))

        #Some defaults to prevent problems
        xSteps=0
        ySteps=0
        zSteps=0
        eSteps=0
        eMotor=""

        if (x is not None):
            xSteps = int(fabs(x*(self.stepsPerMm["X"])))
            dist=xSteps/(self.stepsPerMm["X"])
            if (x<0):
                dist=dist*-1
            self.motorPos["X"]+=dist
        if (y is not None):
            ySteps = int(fabs(y*(self.stepsPerMm["Y"])))
            dist=ySteps/(self.stepsPerMm["Y"])
            if (y<0):
                dist=dist*-1
            self.motorPos["Y"]+=dist
        if (z is not None):
            zSteps = int(fabs(z*(self.stepsPerMm["Z"])))
            dist=zSteps/(self.stepsPerMm["Z"])
            if (z<0):
                dist=dist*-1
            self.motorPos["Z"]+=dist

        self.trace.trace(2,"X:"+str(self.motorPos["X"])+\
                         "   Y:"+str(self.motorPos["Y"])+\
                         "   Z:"+str(self.motorPos["Z"]))
        if (e is not None):
            if (self.tool == 0):
                eStepsPerMm = self.stepsPerMm["E0"]
                self.motorPos["E0"]+=e
                eMotor="E0"
            elif (self.tool==1):
                eStepsPerMm = self.stepsPerMm["E1"]
                self.motorPos["E1"]+=e
                eMotor="E1"
            else:
                eStepsPerMm = 0  
            eSteps = fabs(e*(eStepsPerMm))
        

        self.outputSR.setPinValue(self.motors["Y"]["En"],False)
        self.outputSR.setPinValue(self.motors["Y"]["Dir"],y<0)
        self.outputSR.setPinValue(self.motors["X"]["En"],False)
        self.outputSR.setPinValue(self.motors["X"]["Dir"],x>0)
        self.outputSR.setPinValue(self.motors["Z"]["En"],False)
        self.outputSR.setPinValue(self.motors["Z"]["Dir"],z<0)
        if (e is not None):
            self.outputSR.setPinValue(self.motors[eMotor]["En"],False)
            self.outputSR.setPinValue(self.motors[eMotor]["Dir"],e>0)
        self.outputSR.shiftAll()

        self.trace.trace(2,"Num X Steps: "+str(xSteps)+"  Num Y Steps: "+str(ySteps)+"   Num Z Steps:"+str(zSteps)+"   Num E Steps:"+str(eSteps))

        
        #2D Bresenham's Line Algorithm, for angled lines
        #Doesn't allow for E movement...
        #see www.willperone.net/Code/codeline.php
        #    steep=0
        #    y=0
        #    x=0
        #    if (x is not None):
        #        dx = xSteps
        #    else:
        #        dx = 0
        #    sx=1

        #    if (y is not None):
        #        dy=ySteps
        #    else:
        #        dy=0
        #    sy=1

        #    if dy>dx:
        #        steep=1
        #        x,y = y,x
        #        dx,dy = dy,dx
        #        sx,sy = sy,sx
        #    d = (2*dy)-dx
        #    for i in range(0,dx):
        #        while d>=0:
        #            y=y+sy
        #            if steep:
        #                self.step("X")
        #            else:
        #                self.step("Y")
        #            d = d - (2*dx)
        #        x = x + sx
        #        if steep:
        #            self.step("Y")
        #        else:
        #            self.step("X")
        #        d = d + (2 * dy)
        #                                

        #4D Bresenham Line Drawing, allows for X,Y,Z,E movement
        #See http://www.cb.uu.se/~cris/blog/index.php/archives/400
        #In this case, p1 is always 0,0,0,0
        p = (0,0,0,0)
        d = (xSteps,ySteps,zSteps,eSteps)
        N = round(max(d))
        if (N != 0):
            s = (xSteps/N,ySteps/N,zSteps/N,eSteps/N)
            self.trace.trace(3,"Amount to move: "+str(d)+"  increment: "+str(s))
            for i in range(0,int(N)):
                oldP=list(p) #Avoid aliasing
                p = (p[0]+s[0],p[1]+s[1],p[2]+s[2],p[3]+s[3])
                numXSteps = round(p[0])-round(oldP[0])
                numYSteps = round(p[1])-round(oldP[1])
                numZSteps = round(p[2])-round(oldP[2])
                numESteps = round(p[3])-round(oldP[3])

                if (numXSteps!=0):
                    self.step("X")
              
                if (numYSteps!=0):
                    self.step("Y")
                
                if (numZSteps!=0):
                    self.step("Z")
                
                if (numESteps!=0):
                    self.step(eMotor)
                

        #Do Z Movement
        #if (z is not None):
        #    self.trace.trace(2,"Num Z Steps: "+str(zSteps))
        #    for z in range(0,zSteps):
        #        #self.outputSR.setPinValue(self.motors["Z"]["Step"],True)
        #        #self.outputSR.shiftAll()
        #        #self.outputSR.setPinValue(self.motors["Z"]["Step"],False)
        #        #self.outputSR.shiftAll()
        #        self.step("Z")

        
        return "ok"
